/*
 * Copyright(C) 2022. Huawei Technologies Co.,Ltd. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cppv2.h"
#include "MxBase/Log/Log.h"
#include <algorithm>
#include <map>
#include <numeric>
#include "MxBase/Maths/FastMath.h"
#include "MxBase/Tensor/TensorBase/TensorBase.h"
#include <fstream>
#include <iostream>

using namespace MxBase;
using namespace std;
using namespace Openpose;
using namespace cv;
namespace
{
  std::uint32_t srcheight;
  std::uint32_t srcwidth;
  auto g_uint8Deleter = [](uint8_t *p) {};
  const int K_NUM_BODY_PARTS = 18;
  const int K_UPSAMPLED_STRIDE = 8;

  // CocoSkeletonsNetwork
  const std::vector<unsigned int> K_POSE_MAP_INDEX = {12, 13, 20, 21, 14, 15, 16, 17, 22, 23, 24, 25, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 28, 29, 30, 31, 34, 35, 32, 33, 36, 37, 18, 19, 26, 27};
  // CocoSkeletons
  const std::vector<unsigned int> K_POSE_BODY_PART_SKELETONS = {1, 2, 1, 5, 2, 3, 3, 4, 5, 6, 6, 7, 1, 8, 8, 9, 9, 10, 1, 11, 11, 12, 12, 13, 1, 0, 0, 14, 14, 16, 0, 15, 15, 17, 2, 16, 5, 17};
  const uint32_t YUV_BYTE_NU = 3;
  const uint32_t YUV_BYTE_DE = 2;
  const uint32_t VPC_H_ALIGN = 2;
  const uint32_t OP_RESIZE = 560;
  // Nms score threshold
  const float K_NMS_THRESHOLD = 0.1;
  // Range of nearest neighbors
  const int K_NEAREST_KEYPOINTS_THRESHOLD = 6;
  // PAF score threshold as a valid inner point on a skeleton
  const float K_LOCAL_PAF_SCORE_THRESHOLD = 0.05;
  // The minimum number of valid inner points a skeleton includes to be regarded as a correct skeleton
  const int K_LOCAL_PAF_COUNT_THRESHOLD = 8;
  // The minimum number of skeletons needed to form a person
  const int K_PERSON_SKELETON_COUNT_THRESHOLD = 3;
  // The lowest average score per keypoint in a person
  const float K_PERSON_KEYPOINT_AVG_SCORE_THRESHOLD = 0.2;
}

OpV2Cppv2::OpV2Cppv2(const V2Param &v2Param)
{
  deviceId = v2Param.deviceId;
  std::string modelPath = v2Param.modelPath;
  APP_ERROR ret;

  // global init
  ret = MxInit();
  if (ret != APP_ERR_OK)
  {
    LogError << "MxInit failed, ret=" << ret << ".";
  }

  // imageProcess init
  imageProcessorDptr = std::make_shared<MxBase::ImageProcessor>(deviceId);
  if (imageProcessorDptr == nullptr)
  {
    LogError << "imageProcessorDptr nullptr";
  }

  // model init
  OpV2Dptr = std::make_shared<MxBase::Model>(modelPath, deviceId);
  if (OpV2Dptr == nullptr)
  {
    LogError << "OpV2Dptr nullptr";
  }
};

APP_ERROR OpV2Cppv2::GetImage(const std::string &imgPath, std::shared_ptr<uint8_t> &dataPtr, uint32_t &dataSize)
{
  // Get image data to memory
  std::ifstream file;
  file.open(imgPath.c_str(), std::ios::binary);
  if (!file)
  {
    LogError << "Invalid file.";
    return APP_ERR_COMM_INVALID_PARAM;
  }

  std::stringstream buffer;
  buffer << file.rdbuf();
  std::string content = buffer.str();

  char *p = (char *)malloc(content.size());
  memcpy(p, content.data(), content.size());
  auto deleter = [](void *p) -> void
  {
    free(p);
    p = nullptr;
  };

  dataPtr.reset(static_cast<uint8_t *>((void *)(p)), deleter);
  dataSize = content.size();

  file.close();
  return APP_ERR_OK;
};

APP_ERROR OpV2Cppv2::ReadImage(const std::string &imgPath, MxBase::Image &decodedImage)
{
  APP_ERROR ret;

#ifdef USE_200DK
  std::shared_ptr<uint8_t> dataPtr;
  uint32_t dataSize;
  ret = GetImage(imgPath, dataPtr, dataSize);
  if (ret != APP_ERR_OK)
  {
    LogError << "Get image failed, ret=" << ret;
    return ret;
  }
  ret = OpV2Cppv2::imageProcessorDptr->Decode(dataPtr, dataSize, decodedImage, ImageFormat::YUV_SP_420);
#else
  ret = OpV2Cppv2::imageProcessorDptr->Decode(imgPath, decodedImage, ImageFormat::YUV_SP_420);
#endif
  if (ret != APP_ERR_OK)
  {
    LogError << "Decode failed, ret=" << ret;
    return ret;
  }
  srcheight = decodedImage.GetOriginalSize().height;
  srcwidth = decodedImage.GetOriginalSize().width;
  return APP_ERR_OK;
};

APP_ERROR OpV2Cppv2::OpInfer(MxBase::Image &resizeImage, std::vector<MxBase::Tensor> &OpV2Outputs)
{
  APP_ERROR ret;
  // !move image to device!
  MxBase::Tensor tensorImg = resizeImage.ConvertToTensor();
  ret = tensorImg.ToDevice(deviceId);
  if (ret != APP_ERR_OK)
  {
    LogError << "ToDevice failed, ret=" << ret;
    return ret;
  }

  // make infer input
  std::vector<MxBase::Tensor> OpV2Inputs = {tensorImg};
  // do infer
  OpV2Outputs = OpV2Dptr->Infer(OpV2Inputs);

  // !move result to host!
  for (auto output : OpV2Outputs)
  {
    output.ToHost();
  }
  return APP_ERR_OK;
};

static std::vector<std::vector<cv::Mat>> ReadDataFromTensorPytorch(const std::vector<MxBase::Tensor> &tensors)
{
  const int heightIndex = 2, widthIndex = 3;
  auto shape = tensors[0].GetShape();
  int channelKeypoint = shape[1];
  int height = shape[heightIndex];
  int width = shape[widthIndex];
  auto shapeP = tensors[1].GetShape();
  int channelPaf = shapeP[1];
  // Read keypoint data
  auto dataPtr = (uint8_t *)tensors[0].GetData();
  std::shared_ptr<void> keypointPointer;
  keypointPointer.reset(dataPtr, g_uint8Deleter);
  std::vector<cv::Mat> keypointHeatmap = {};
  int idx = 0;
  for (int i = 0; i < channelKeypoint; i++)
  {
    cv::Mat singleChannelMat(height, width, CV_32FC1, cv::Scalar(0));
    for (int j = 0; j < height; j++)
    {
      float *ptr = singleChannelMat.ptr<float>(j);
      for (int k = 0; k < width; k++)
      {
        ptr[k] = static_cast<float *>(keypointPointer.get())[idx];
        idx += 1;
      }
    }
    keypointHeatmap.push_back(singleChannelMat);
  }
  // Read PAF data
  auto dataPafPtr = (uint8_t *)tensors[1].GetData();
  std::shared_ptr<void> pafPointer;
  pafPointer.reset(dataPafPtr, g_uint8Deleter);
  std::vector<cv::Mat> pafHeatmap = {};
  idx = 0;
  for (int i = 0; i < channelPaf; i++)
  {
    cv::Mat singleChannelMat(height, width, CV_32FC1, cv::Scalar(0));
    for (int j = 0; j < height; j++)
    {
      float *ptr = singleChannelMat.ptr<float>(j);
      for (int k = 0; k < width; k++)
      {
        ptr[k] = static_cast<float *>(pafPointer.get())[idx];
        idx += 1;
      }
    }
    pafHeatmap.push_back(singleChannelMat);
  }
  std::vector<std::vector<cv::Mat>> result = {keypointHeatmap, pafHeatmap};

  return result;
}

bool OpV2Cppv2::MergeSkeletonToPerson(std::vector<std::vector<PartPair>> &personList,
                                      PartPair currentPair)
{
  // Use point1 and point2 to represent the two endpoints of a skeleton
  for (int k = 0; k < int(personList.size()); k++)
  {
    std::vector<PartPair> &currentPerson = personList[k];
    for (int i = 0; i < int(currentPerson.size()); i++)
    {
      if (currentPair.partIdx1 == currentPerson[i].partIdx1 &&
          currentPair.idx1 == currentPerson[i].idx1)
      {
        // point1 of current skeleton is the same as point1 of a skeleton in current person
        currentPerson.push_back(currentPair);
        return true;
      }
      else if (currentPair.partIdx1 == currentPerson[i].partIdx2 &&
               currentPair.idx1 == currentPerson[i].idx2)
      {
        // point1 of current skeleton is the same as point2 of a skeleton in current person
        currentPerson.push_back(currentPair);
        return true;
      }
      else if (currentPair.partIdx2 == currentPerson[i].partIdx1 &&
               currentPair.idx2 == currentPerson[i].idx1)
      {
        // point2 of current skeleton is the same as point1 of a skeleton in current person
        currentPerson.push_back(currentPair);
        return true;
      }
      else if (currentPair.partIdx2 == currentPerson[i].partIdx2 &&
               currentPair.idx2 == currentPerson[i].idx2)
      {
        // point2 of current skeleton is the same as point2 of a skeleton in current person
        currentPerson.push_back(currentPair);
        return true;
      }
    }
  }
  // Can not merge to any existed person, create new person
  std::vector<PartPair> newPerson = {};
  newPerson.push_back(currentPair);
  personList.push_back(newPerson);
  return true;
}

float OpV2Cppv2::PersonScore(const std::vector<PartPair> &person)
{
  // The score of a person is composed of the scores of all his keypoints and that of all his skeletons
  PartPair part;
  std::vector<int> seenKeypoints = {};
  float personScore = 0.0;
  for (size_t i = 0; i < person.size(); i++)
  {
    PartPair skeleton = person[i];
    if (std::find(seenKeypoints.begin(), seenKeypoints.end(), skeleton.partIdx1) == seenKeypoints.end())
    {
      seenKeypoints.push_back(skeleton.partIdx1);
      personScore += skeleton.score1;
    }
    if (std::find(seenKeypoints.begin(), seenKeypoints.end(), skeleton.partIdx2) == seenKeypoints.end())
    {
      seenKeypoints.push_back(skeleton.partIdx2);
      personScore += skeleton.score2;
    }
    personScore += skeleton.score;
  }
  // Ignore person whose number of skeletons is less than K_PERSON_SKELETON_COUNT_THRESHOLD or
  // the average score of each keypoint is less than K_PERSON_KEYPOINT_AVG_SCORE_THRESHOLD
  if (seenKeypoints.size() < K_PERSON_SKELETON_COUNT_THRESHOLD ||
      (personScore / seenKeypoints.size()) < K_PERSON_KEYPOINT_AVG_SCORE_THRESHOLD)
  {
    return 0.0;
  }
  return personScore;
}

static bool PointSort(cv::Point p1, cv::Point p2)
{
  return p1.x < p2.x;
}

bool OpV2Cppv2::GreaterSort(PartPair p1, PartPair p2)
{
  return p1.score > p2.score;
}
static void NMS(cv::Mat &plain, float threshold)
{
  // Keep points with score below the NMS score threshold are set to 0
  plain.setTo(0, plain < threshold);
  cv::Mat plainWithBorder;
  const int borderPadding = 2;
  const int bottomRightIndex = 2;
  cv::copyMakeBorder(plain, plainWithBorder, borderPadding, borderPadding, borderPadding, borderPadding,
                     BORDER_CONSTANT, cv::Scalar(0));
  cv::Mat plainWithBorderClone = plainWithBorder.clone();
  int subMatCols = plainWithBorder.cols - borderPadding;
  int subMatRows = plainWithBorder.rows - borderPadding;
  cv::Mat plainCenter = plainWithBorder(cv::Rect(1, 1, subMatCols, subMatRows));
  cv::Mat plainBottom = plainWithBorder(cv::Rect(1, bottomRightIndex, subMatCols, subMatRows));
  cv::Mat plainUp = plainWithBorder(cv::Rect(1, 0, subMatCols, subMatRows));
  cv::Mat plainLeft = plainWithBorder(cv::Rect(0, 1, subMatCols, subMatRows));
  cv::Mat plainRight = plainWithBorder(cv::Rect(bottomRightIndex, 1, subMatCols, subMatRows));
  for (int i = 0; i < plainCenter.rows; i++)
  {
    float *centerPtr = plainCenter.ptr<float>(i);
    float *bottomPtr = plainBottom.ptr<float>(i);
    float *upPtr = plainUp.ptr<float>(i);
    float *leftPtr = plainLeft.ptr<float>(i);
    float *rightPtr = plainRight.ptr<float>(i);
    float *cloneBorderPtr = plainWithBorderClone.ptr<float>(i + 1);
    for (int j = 0; j < plainCenter.cols; j++)
    {
      if (!((centerPtr[j] > upPtr[j]) && (centerPtr[j] > bottomPtr[j]) &&
            (centerPtr[j] > leftPtr[j]) && (centerPtr[j] > rightPtr[j])))
      {
        cloneBorderPtr[j + 1] = 0;
      }
    }
  }
  plain = plainWithBorderClone(cv::Rect(borderPadding, borderPadding, plainCenter.cols - borderPadding,
                                        plainCenter.rows - borderPadding))
              .clone();
}

APP_ERROR OpV2Cppv2::ExtractKeypoints(std::vector<cv::Mat> &keypointHeatmap,
                                      std::vector<std::vector<cv::Point>> &coor,
                                      std::vector<std::vector<float>> &coorScore)
{
  const int polynomialExponent = 2;
  for (size_t i = 0; i < keypointHeatmap.size() - 1; i++)
  {
    NMS(keypointHeatmap[i], K_NMS_THRESHOLD);

    std::vector<cv::Point> nonZeroCoordinates;
    cv::findNonZero(keypointHeatmap[i], nonZeroCoordinates);
    std::sort(nonZeroCoordinates.begin(), nonZeroCoordinates.end(), PointSort);
    std::vector<int64_t> suppressed(nonZeroCoordinates.size(), 0);
    std::vector<cv::Point> keypointsWithoutNearest = {};
    std::vector<float> keypointsScore = {};
    // Remove other keypoints within a certain range around one keypoints
    for (size_t j = 0; j < nonZeroCoordinates.size(); j++)
    {
      if (suppressed[j])
      {
        continue;
      }
      int thrownIndex = j + 1;
      auto it = std::find_if(std::begin(nonZeroCoordinates) + j + 1, std::end(nonZeroCoordinates),
                             [nonZeroCoordinates, j, polynomialExponent](cv::Point p)
                             {
                               float distance = powf((nonZeroCoordinates[j].x - p.x), polynomialExponent) +
                                                powf((nonZeroCoordinates[j].y - p.y), polynomialExponent);
                               return sqrtf(distance) < K_NEAREST_KEYPOINTS_THRESHOLD;
                             });
      while (it != std::end(nonZeroCoordinates))
      {
        thrownIndex = std::distance(std::begin(nonZeroCoordinates) + thrownIndex, it) + thrownIndex;
        suppressed[thrownIndex] = 1;
        it = std::find_if(std::next(it), std::end(nonZeroCoordinates),
                          [nonZeroCoordinates, j, polynomialExponent](cv::Point p)
                          {
                            float distance = powf((nonZeroCoordinates[j].x - p.x), polynomialExponent) +
                                             powf((nonZeroCoordinates[j].y - p.y), polynomialExponent);
                            return sqrtf(distance) < K_NEAREST_KEYPOINTS_THRESHOLD;
                          });
      }
      keypointsWithoutNearest.push_back(nonZeroCoordinates[j]);
      keypointsScore.push_back(keypointHeatmap[i].at<float>(
          nonZeroCoordinates[j].y, nonZeroCoordinates[j].x));
    }

    coor.push_back(keypointsWithoutNearest);
    coorScore.push_back(keypointsScore);
  }

  return APP_ERR_OK;
}

APP_ERROR OpV2Cppv2::GeneratePersonList(std::vector<MxBase::Tensor> &tensors, std::vector<std::vector<PartPair>> &personList,
                                        mxpiopenposeproto::MxpiPersonList &dstMxpiPersonList, bool isEval) // mxpiopenposeproto::MxpiPersonList need change to user define struct
{
  // Read data
  std::vector<cv::Mat> keypointHeatmap, pafHeatmap;
  std::vector<int> visionInfos = {};
  visionInfos.push_back(srcheight);
  visionInfos.push_back(srcwidth);
  visionInfos.push_back(OP_RESIZE);
  visionInfos.push_back(OP_RESIZE);
  std::vector<std::vector<cv::Mat>> result = ReadDataFromTensorPytorch(tensors);
  keypointHeatmap = result[0];
  pafHeatmap = result[1];
  // Resize heatmaps to the size of the input image
  ResizeHeatmaps(keypointHeatmap, pafHeatmap, visionInfos);
  // Extract candidate keypoints
  std::vector<std::vector<cv::Point>> coor = {};
  std::vector<std::vector<float>> coorScore = {};
  ExtractKeypoints(keypointHeatmap, coor, coorScore);
  // Group candidate keypoints to candidate skeletons and generate person
  GroupKeypoints(pafHeatmap, coor, coorScore, personList);
  // Prepare output in the format of MxpiPersonList
  GenerateMxpiOutput(personList, dstMxpiPersonList, isEval);
  return APP_ERR_OK;
}

APP_ERROR OpV2Cppv2::ResizeHeatmaps(std::vector<cv::Mat> &keypointHeatmap,
                                    std::vector<cv::Mat> &pafHeatmap,
                                    std::vector<int> &visionInfos)
{
  inputHeight_ = OP_RESIZE;
  inputWidth_ = OP_RESIZE;
  // Calculate padding direction and padding value
  int originHeight = visionInfos[0];
  int originWidth = visionInfos[1];
  // padding along height
  int paddingDirection = 0;
  if (originHeight > originWidth)
  {
    // padding along width
    paddingDirection = 1;
  }
  int paddingValue = 0;
  if (paddingDirection == 0)
  {
    // pad height
    paddingValue = floor(inputHeight_ - inputWidth_ * originHeight / originWidth);
  }
  else
  {
    // pad width
    paddingValue = floor(inputWidth_ - inputHeight_ * originWidth / originHeight);
  }
  // Channel Split Resize

  for (int i = 0; i < int(keypointHeatmap.size()); i++)
  {
    cv::Mat singleChannelMat = keypointHeatmap[i];
    cv::resize(singleChannelMat, singleChannelMat, cv::Size(0, 0),
               K_UPSAMPLED_STRIDE, K_UPSAMPLED_STRIDE, 1);
    if (paddingDirection == 0)
    {
      // remove height padding
      singleChannelMat =
          singleChannelMat(cv::Rect(0, 0, singleChannelMat.cols, singleChannelMat.rows - paddingValue));
    }
    else
    {
      // remove width padding
      singleChannelMat =
          singleChannelMat(cv::Rect(0, 0, singleChannelMat.cols - paddingValue, singleChannelMat.rows));
    }
    cv::resize(singleChannelMat, singleChannelMat, cv::Size(originWidth, originHeight), 0, 0, 1);
    keypointHeatmap[i] = singleChannelMat;
  }
  for (int i = 0; i < int(pafHeatmap.size()); i++)
  {
    cv::Mat singleChannelMat = pafHeatmap[i];
    cv::resize(singleChannelMat, singleChannelMat, cv::Size(0, 0),
               K_UPSAMPLED_STRIDE, K_UPSAMPLED_STRIDE, 1);
    if (paddingDirection == 0)
    {
      singleChannelMat =
          singleChannelMat(cv::Rect(0, 0, singleChannelMat.cols, singleChannelMat.rows - paddingValue));
    }
    else
    {
      singleChannelMat =
          singleChannelMat(cv::Rect(0, 0, singleChannelMat.cols - paddingValue, singleChannelMat.rows));
    }
    cv::resize(singleChannelMat, singleChannelMat, cv::Size(originWidth, originHeight), 0, 0, 1);
    pafHeatmap[i] = singleChannelMat;
  }
  return APP_ERR_OK;
}

APP_ERROR OpV2Cppv2::GroupKeypoints(const std::vector<cv::Mat> &pafHeatmap,
                                    const std::vector<std::vector<cv::Point>> &coor,
                                    const std::vector<std::vector<float>> &coorScore,
                                    std::vector<std::vector<PartPair>> &personList)
{
  for (int i = 0; i < K_NUM_BODY_PARTS + 1; i++)
  {
    // Chooose candidate skeletons for each category, there are a total of kNumBodyPart + 1 categories of skeletons
    std::vector<PartPair> partConnections;
    ScoreSkeletons(i, coor, coorScore, pafHeatmap, partConnections);
    // Merge newly generated skeletons to existed person or create new person
    if (i == 0)
    {
      // For the first category, each different skeleton of this category stands for different person
      for (size_t j = 0; j < partConnections.size(); j++)
      {
        std::vector<PartPair> newPerson;
        newPerson.push_back(partConnections[j]);
        personList.push_back(newPerson);
      }
    }
    else if (i == K_NUM_BODY_PARTS - 1 || i == K_NUM_BODY_PARTS)
    {
      // The last two skeletons do not contribute to person score
      for (size_t j = 0; j < partConnections.size(); j++)
      {
        partConnections[j].score = 0;
        partConnections[j].score1 = 0;
        partConnections[j].score2 = 0;
        MergeSkeletonToPerson(personList, partConnections[j]);
      }
    }
    else
    {
      for (size_t j = 0; j < partConnections.size(); j++)
      {
        MergeSkeletonToPerson(personList, partConnections[j]);
      }
    }
  }
  return APP_ERR_OK;
}

APP_ERROR OpV2Cppv2::GenerateMxpiOutput(const std::vector<std::vector<PartPair>> &personList,
                                        mxpiopenposeproto::MxpiPersonList &dstMxpiPersonList, bool isEval)
{
  const float floatEqualZeroBias = 0.000001;
  ofstream outfile;
  outfile.open("output_V2.txt", ios::app);
  for (size_t k = 0; k < personList.size(); k++)
  {
    float personScore = PersonScore(personList[k]);
    if (fabs(personScore - 0) < floatEqualZeroBias)
    {
      continue;
    }
    auto mxpiPersonPtr = dstMxpiPersonList.add_personinfovec();
    mxpiopenposeproto::MxpiMetaHeader *dstPersonMxpiMetaheaderList = mxpiPersonPtr->add_headervec();
    dstPersonMxpiMetaheaderList->set_datasource(parentName_);
    dstPersonMxpiMetaheaderList->set_memberid(0);
    mxpiPersonPtr->set_score(personScore);
    for (size_t j = 0; j < personList[k].size(); j++)
    {
      PartPair skeleton = personList[k][j];
      auto mxpiSkeletonPtr = mxpiPersonPtr->add_skeletoninfovec();
      mxpiSkeletonPtr->set_cocoskeletonindex1(skeleton.partIdx1);
      mxpiSkeletonPtr->set_cocoskeletonindex2(skeleton.partIdx2);
      mxpiSkeletonPtr->set_x0(skeleton.coord1[0]);
      mxpiSkeletonPtr->set_y0(skeleton.coord1[1]);
      mxpiSkeletonPtr->set_point1score(skeleton.score1);
      mxpiSkeletonPtr->set_x1(skeleton.coord2[0]);
      mxpiSkeletonPtr->set_y1(skeleton.coord2[1]);
      mxpiSkeletonPtr->set_point2score(skeleton.score2);
      mxpiSkeletonPtr->set_skeletonscore(skeleton.score);
      if (isEval)
      {
        outfile << skeleton.score << "\n";
        outfile.close();
      }
    }
  }
  return APP_ERR_OK;
}

APP_ERROR OpV2Cppv2::ScoreSkeletons(const int partIndex,
                                    const std::vector<std::vector<cv::Point>> &coor,
                                    const std::vector<std::vector<float>> &coorScore,
                                    const std::vector<cv::Mat> &pafHeatmap,
                                    std::vector<PartPair> &connections)
{
  // Use point1 and point2 to represent the two endpoints of a skeleton
  const int indexStride = 2;
  const int endPointNum = 2;
  int cocoSkeletonIndex1 = K_POSE_BODY_PART_SKELETONS[indexStride * partIndex];
  int cocoSkeletonIndex2 = K_POSE_BODY_PART_SKELETONS[indexStride * partIndex + 1];
  int pafXIndex = K_POSE_MAP_INDEX[indexStride * partIndex];
  int pafYIndex = K_POSE_MAP_INDEX[indexStride * partIndex + 1];
  std::vector<cv::Point> endpoints(endPointNum, cv::Point(0, 0));
  std::vector<PartPair> connectionTemp = {};
  std::vector<float> result = {0.0, 0.0};

  for (int i = 0; i < int(coor[cocoSkeletonIndex1].size()); i++)
  {
    cv::Point point1;
    point1.x = coor[cocoSkeletonIndex1][i].x;
    point1.y = coor[cocoSkeletonIndex1][i].y;
    endpoints[0] = point1;
    for (int j = 0; j < int(coor[cocoSkeletonIndex2].size()); j++)
    {
      cv::Point point2;
      point2.x = coor[cocoSkeletonIndex2][j].x;
      point2.y = coor[cocoSkeletonIndex2][j].y;
      endpoints[1] = point2;
      result = OneSkeletonScore(endpoints, pafHeatmap[pafXIndex], pafHeatmap[pafYIndex]);
      // Keep skeletons with affinity scores greater than 0 and
      // valid internal points greater than K_LOCAL_PAF_COUNT_THRESHOLD
      if (result[1] <= K_LOCAL_PAF_COUNT_THRESHOLD || result[0] <= 0.0)
      {
        continue;
      }
      // Store the information of a skeleton in a custom structure PartPair
      PartPair skeleton;
      skeleton.score = result[0];
      skeleton.partIdx1 = cocoSkeletonIndex1;
      skeleton.partIdx2 = cocoSkeletonIndex2;
      skeleton.idx1 = i;
      skeleton.idx2 = j;
      skeleton.coord1.push_back(point1.x);
      skeleton.coord1.push_back(point1.y);
      skeleton.coord2.push_back(point2.x);
      skeleton.coord2.push_back(point2.y);
      skeleton.score1 = coorScore[cocoSkeletonIndex1][i];
      skeleton.score2 = coorScore[cocoSkeletonIndex2][j];
      connectionTemp.push_back(skeleton);
    }
  }
  // For skeletons with the same endpoints, keep the one with larger affinity score
  ConntectionNms(connectionTemp, connections);
  return APP_ERR_OK;
}

std::vector<float> OpV2Cppv2::OneSkeletonScore(std::vector<cv::Point> &endpoints,
                                               const cv::Mat &pafX, const cv::Mat &pafY)
{
  int x1 = endpoints[0].x, y1 = endpoints[0].y;
  int x2 = endpoints[1].x, y2 = endpoints[1].y;
  // affinity score of this skeleton
  float score = 0;
  // count: number of valid inner points on this skeleton
  int count = 0, numInter = 10;
  float dx = x2 - x1;
  float dy = y2 - y1;
  float normVec = sqrt(dx * dx + dy * dy);
  float vx = dx / (normVec + 1e-6);
  float vy = dy / (normVec + 1e-6);
  // generate 10 points equally spaced on this skeleton
  std::vector<int> xs = {};
  float stepX = dx / (numInter - 1);
  float tempX = 0;
  for (int k = 0; k < numInter; k++)
  {
    tempX = x1 + k * stepX;
    xs.push_back(round(tempX));
  }
  std::vector<int> ys = {};
  float stepY = dy / (numInter - 1);
  float tempY = 0;
  for (int k = 0; k < numInter; k++)
  {
    tempY = y1 + k * stepY;
    ys.push_back(round(tempY));
  }
  std::vector<float> subScoreVec;
  // calculate PAF value of each inner point
  float subScore = 0.0;
  for (int i = 0; i < int(xs.size()); i++)
  {
    subScore = pafX.at<float>(ys[i], xs[i]) * vx + pafY.at<float>(ys[i], xs[i]) * vy;
    subScoreVec.push_back(subScore);
  }
  // remove inner points such that has PAF value < K_LOCAL_PAF_SCORE_THRESHOLD
  subScoreVec.erase(std::remove_if(subScoreVec.begin(), subScoreVec.end(), [](const float &x)
                                   { return x <= K_LOCAL_PAF_SCORE_THRESHOLD; }),
                    subScoreVec.end());
  std::vector<float> result = {0.0, 0.0};
  score = std::accumulate(subScoreVec.begin(), subScoreVec.end(), 0.0);
  count = subScoreVec.size();
  result[0] = score / (count + 1e-6);
  result[1] = count;
  return result;
}

APP_ERROR OpV2Cppv2::ConntectionNms(std::vector<PartPair> &src, std::vector<PartPair> &dst)
{
  // Remove conflict skeletons, if two skeletons of the same type share a same end point, they are conflict
  std::vector<int> usedIndex1 = {};
  std::vector<int> usedIndex2 = {};
  // Sort skeletons in ascending order of affinity score
  std::sort(src.begin(), src.end(), [&](PartPair i, PartPair j)
            { return i.score > j.score; });
  for (int i = 0; i < int(src.size()); i++)
  {
    PartPair candidate = src[i];
    if (std::find(usedIndex1.begin(), usedIndex1.end(), candidate.idx1) != usedIndex1.end() || std::find(usedIndex2.begin(), usedIndex2.end(), candidate.idx2) != usedIndex2.end())
    {
      continue;
    }
    dst.push_back(candidate);
    usedIndex1.push_back(candidate.idx1);
    usedIndex2.push_back(candidate.idx2);
  }
  return APP_ERR_OK;
}
